from Maix import MIC_ARRAY as mic
from Maix import GPIO
import lcd,time,image
import math
import utime
from Maix import GPIO
from board import board_info
from fpioa_manager import fm
from machine import Timer,PWM
import time

mic.init(sk9822_dat=9,i2s_d1=10,i2s_d3=11,i2s_sclk=12,sk9822_clk=20,i2s_d0=15,i2s_d2=19,i2s_ws=18)#默认配置
lcd.init()   #320*240
#mic.init(i2s_d0=34, i2s_d1=8, i2s_d2=33, i2s_d3=9, i2s_ws=32, i2s_sclk=10,\
            #sk9822_dat=7, sk9822_clk=35)#可自定义配置 IO
io_led_red = 6
fm.register(io_led_red, fm.fpioa.GPIO0)#配置红外的GPIO
HY=GPIO(GPIO.GPIO0, GPIO.OUT,value=0)

fm.register(26, fm.fpioa.GPIO1)#配置两个开关的GPIO   是0和2脚 01是第二题 10第三题 11第四题
KG0=GPIO(GPIO.GPIO1, GPIO.IN, GPIO.PULL_DOWN)
                                                 #建议拿第四题测试
fm.register(27, fm.fpioa.GPIO2)#配置两个开关的GPIO   是0和2脚 01是第二题 10第三题 11第四题
KG1=GPIO(GPIO.GPIO2, GPIO.IN, GPIO.PULL_DOWN)
#KG0.value(1)
#image.font_load(image.UTF8, 16, 16, '/sd/0xA00000_font_uincode_16_16_tblr.Dzk')
#image.font_load(image.UTF8, 16, 16, 0xA00000)   #加载字库
tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
S1 = PWM(tim, freq=50, duty=0, pin=17)#舵机初始化
img = image.Image()
#lcd.draw_string(70, 200, "Angle: " + str(float(-Angle)), lcd.BLUE, lcd.BLACK)
if (KG0.value()==0)and(KG1.value()==1) :   ##显示汉字的
    lcd.draw_string(60, 70, "Two" , lcd.BLUE, lcd.BLACK)
                                 #引脚0和2的电平是10代表第三题
if (KG0.value()==1)and(KG1.value()==0) :
    lcd.draw_string(60, 70, "Three" , lcd.BLUE, lcd.BLACK)
                                 ##引脚0和2的电平是11代表第四题
if (KG0.value()==1)and(KG1.value()==1) :
    lcd.draw_string(60, 70, "Four" , lcd.BLUE, lcd.BLACK)
tt=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
tt_last=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
num=0
num2=0
Angle_LB = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
t=0
t1=0
t2=0
maxnum=0
minnum=0
jiaodu=0
Angle_last=0
b=[3,1,2,5,6]
i=100
a=[]
Biaozhi2=0    #函数2标志位
cishu=0
pid=0
err=0
JD=0
#以上变量都是给函数用的工具人，不用调用或者读取，没用

##########################卡尔曼##############################
KF_lastP=0.1  #上次的协方差
KF_nowP=0   #本次的协方差
KF_x_hat=0     #卡尔曼滤波的计算值，即为后验最优值
KF_Kg=0.1        #卡尔曼增益系数
KF_Q=0.3         #过程噪声
KF_R=0.3         #测量噪声
##################################################卡尔曼##########################
def Kalman_Filter(value):
    global KF_lastP        #上次的协方差
    global KF_nowP         #本次的协方差
    global KF_x_hat        #卡尔曼滤波的计算值，即为后验最优值
    global KF_Kg           #卡尔曼增益系数
    global KF_Q            #过程噪声
    global KF_R            #测量噪声
    output=0        #output为卡尔曼滤波计算值
    x_t=KF_x_hat    #当前先验预测值 = 上一次最优值
    KF_nowP=KF_lastP+KF_Q       #本次的协方差矩阵
    KF_Kg=KF_nowP/(KF_nowP+KF_R)#卡尔曼增益系数计算
    output=x_t+KF_Kg*(value-x_t)#当前最优值
    KF_x_hat=output     #更新最优值
    KF_lastP=(1-KF_Kg)*KF_nowP#更新协方差矩阵
    return output

##############################函数0###############################################
def get_med(jiaodu,cs):
    global a
    global num2
    a.append(jiaodu)
    if num2==(cs-1) :
        num2=0
        for j in range(1,len(a)):
            for i in range(len(a)-1):
                if a[i] > a[i+1]:
                    a[i], a[i+1] = a[i+1],a[i]
        t2=a[int((cs-1)/2)]
        a=[]      #清空，准备下一次测量
        return t2

    else :
        num2+=1
        return 1000

##############################函数1###############################################
def get_LB(jd,cs):       #滤波，jd是传入数据，cs是滤波等级要大于2，越高越慢，返回滤波后的数据
    global Angle_LB
    global num
    global minnum
    global jiaodu
    global maxnum
    Angle_LB[num]=jd
    if Angle_LB[num]>Angle_LB[maxnum] : maxnum=num  #取最大最小值的位号
    if Angle_LB[num]<Angle_LB[minnum] : minnum=num


    if num==cs:
       Angle_LB[maxnum]=0                          #去掉最大值最小值
       Angle_LB[minnum]=0
       for i in range(cs+1):
           jiaodu+=Angle_LB[i]
       jiaodu/=(cs-1)
       num=0
       maxnum=0
       minnum=0
       return int(jiaodu)
    else:
        num+=1
        return 1000
##############################函数2##########舵机函数###############################
def Servo(servo,angle):
    S1.duty((angle+90)/180*10+2.5)
##############################函数3######第四题函数###############################
def get_mic_dir4():
    global Angle_LB
    global Angle_last
    global pid
    global JD
    AngleX=0
    AngleY=0
    AngleR=0
    Angle=0
    AngleAddPi=0
    Juli=0
    mic_list=[]
    imga = mic.get_map()    # 获取声音源分布图像(返回声源黑白位图)
    b = mic.get_dir(imga)   # 计算、获取声源方向(从声源位图计算声源方向 返回12个强度值 对应12个LED灯)
    HY.value(1)
    #for i in range(len(b)):
        #tt[i]=b[i]
        #tt[i]=tt[i]*0.7+tt_last[i]*0.3     #低通滤波

    ##tt[9]*=0.4
    ##tt[10]*=0.4
    ##tt[11]*=0.6
    #tt[0]*=1.3
    #tt[1]*=1.4
    #tt[2]*=1
    #for i in range(len(b)):

        #tt_last[i]=tt[i]


    #print(tt[9],tt[10],tt[11],tt[0],tt[1],tt[2],tt[3])
    for i in range(len(b)):
        if b[i]>=0:
            AngleX+= b[i] * math.sin(i * math.pi/6)
            AngleY+= b[i] * math.cos(i * math.pi/6)

    AngleX=round(AngleX,6) #计算坐标转换值
    AngleY=round(AngleY,6)
    if AngleY<0:AngleAddPi=180
    if AngleX<0 and AngleY > 0:AngleAddPi=360
    if AngleX!=0 or AngleY!=0: #参数修正
        if AngleY==0:
            Angle=90 if AngleX>0 else 270 #填补X轴角度
        else:
            Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #计算角度



        if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):
                if Angle>315 and Angle<360:             #处理315-360的角度值
                    Angle=Angle-345

                #if abs(Angle-Angle_last)<30 :           #本次的值和上次的差值小于40才取

                #print(Angle)
                #Angle=Angle_last*0.3+ Angle*0.7      #低通滤波
                #/t=get_LB(Angle,10)
                t= get_LB(Angle,5)           #滤波，jd是传入数据，cs是滤波等级，越高越慢，返回滤波后的数据


                if t !=1000 :
                    #t1=get_med(t,3)    #再取中位数
                    #if t1 !=1000 :
                    if t>30 : t=30         #限幅
                    if t<-30 : t=-30
                    Angle=t
                    Kalman_Filter(Angle)
                    Angle_last=Angle
                    lcd.draw_string(70, 200, "Angle: " + str(float(Angle)), lcd.BLUE, lcd.BLACK)
                    lcd.draw_string(70, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)), lcd.BLUE, lcd.BLACK)
                    lcd.draw_string(70, 100, "X: " + str((((275/math.cos(-Angle*math.pi/180))**2-75625)**0.5)*(Angle+0.001)/abs(Angle+0.001)-8), lcd.BLUE, lcd.BLACK)
                    lcd.draw_string(70, 120, "Y: " + str(275), lcd.BLUE, lcd.BLACK)
                    #Servo(S1,-pid*0.666)
                    lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0))    #清空lcd右边的区域
                    lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*167), 15, 15, (0, 255,200))#角度位置实时
                    JD=Angle


        #AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #计算强度
        #mic_list.append(AngleX)     #X坐标
        #mic_list.append(AngleY)     #Y坐标
        #mic_list.append(AngleR)     #强度
        #mic_list.append(Angle)      #角度

        #Juli=2.5/math.cos(Angle)
        #mic_list.append(Juli)
        #print(Juli)
    a = mic.set_led(b,(10,10,0))# 配置 RGB LED 颜色值   （从计算的声源方向设置点亮对应的LED灯）
    #lcd.display(0,color=(255,0,0))
    #img1=draw_line(100,0,100,240,color=(255,0,0))
    #lcd.display(img1)

    #lcd._line(150,0,150,240,color=(255,0,0))
    #img.draw_rectangle((30,30,50,50), color = (255, 0, 0))


    return mic_list #返回列表，X坐标，Y坐标，强度，角度
##############################函数3######第三题函数###############################
def get_mic_dir3():
    global Angle_LB
    global Angle_last
    global cishu
    AngleX=0
    AngleY=0
    AngleR=0
    Angle=0
    AngleAddPi=0
    Juli=0
    mic_list=[]
    imga = mic.get_map()    # 获取声音源分布图像(返回声源黑白位图)
    b = mic.get_dir(imga)   # 计算、获取声源方向(从声源位图计算声源方向 返回12个强度值 对应12个LED灯)
    #print(b[0],b[1],b[2],b[10],b[11])
    cishu+=1   #判断进入函数的次数
    if cishu==1 :
        HY.value(0)              # 第一次设置红外地线高电平，关闭红外
    for i in range(len(b)):
        if b[i]>=2:
            AngleX+= b[i] * math.sin(i * math.pi/6)
            AngleY+= b[i] * math.cos(i * math.pi/6)
    AngleX=round(AngleX,6) #计算坐标转换值
    AngleY=round(AngleY,6)
    if AngleY<0:AngleAddPi=180
    if AngleX<0 and AngleY > 0:AngleAddPi=360
    if AngleX!=0 or AngleY!=0: #参数修正
        if AngleY==0:
            Angle=90 if AngleX>0 else 270 #填补X轴角度
        else:
            Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #计算角度



        if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):

                if Angle>315 and Angle<360:             #处理330-360的角度值

                    Angle=Angle-335
                if Angle>10 and Angle<45:
                    Angle=Angle+10
                Angle=Angle_last*0.1+ Angle*0.9      #低通滤波

                t=get_LB(Angle,6)           #滤波，jd是传入数据，cs是滤波等级，越高越慢，返回滤波后的数据


                if t !=1000 :
                    t1=get_med(t,3)    #再取中位数
                    if t1 !=1000 :
                        if t1>30 : t1=30
                        if t1<-30 : t1=-30
                        Angle=t1
                        Angle_last=Angle
                        HY.value(1)              # 设置红外地线低电平，开启红外
                        lcd.draw_string(60, 200, "Angle: " + str(float(Angle)), lcd.BLUE, lcd.BLACK)
                        lcd.draw_string(60, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)), lcd.BLUE, lcd.BLACK)
                        #距离只能在比赛规定场地测算，注意声源和系统垂直距离是275cm（D区中间）
                        lcd.draw_string(70, 100, "X: " + str((((275/math.cos(-Angle*math.pi/180))**2-75625)**0.5)*(Angle+0.001)/abs(Angle+0.001)-8), lcd.BLUE, lcd.BLACK)
                        lcd.draw_string(70, 120, "Y: " + str(275), lcd.BLUE, lcd.BLACK)
                        Servo(S1,-Angle+15)
                        lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0))    #清空右边的区域
                        lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*165), 15, 15, (0, 255,200))#角度位置实时



        AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #计算强度
        mic_list.append(AngleX)     #X坐标
        mic_list.append(AngleY)     #Y坐标
        mic_list.append(AngleR)     #强度
        mic_list.append(Angle)      #角度

        #Juli=2.5/math.cos(Angle)
        #mic_list.append(Juli)
        #print(Juli)
    a = mic.set_led(b,(10,10,0))# 配置 RGB LED 颜色值   （从计算的声源方向设置点亮对应的LED灯）
    #lcd.display(0,color=(255,0,0))
    #img1=draw_line(100,0,100,240,color=(255,0,0))
    #lcd.display(img1)

    #lcd._line(150,0,150,240,color=(255,0,0))
    #img.draw_rectangle((30,30,50,50), color = (255, 0, 0))


    return mic_list #返回列表，X坐标，Y坐标，强度，角度

##############################函数5###############################################
def get_mic_dir2():   ######################第二题函数
    global Angle_LB
    global Angle_last
    AngleX=0
    AngleY=0
    AngleR=0
    Angle=0
    AngleAddPi=0
    Juli=0
    mic_list=[]
    imga = mic.get_map()    # 获取声音源分布图像(返回声源黑白位图)
    b = mic.get_dir(imga)   # 计算、获取声源方向(从声源位图计算声源方向 返回12个强度值 对应12个LED灯)
    #print(b[0],b[1],b[2],b[10],b[11])
    Servo(S1,0)       #控制舵机保持中立
    HY.value(0)              # 设置红外地线高电平，关闭红外
    for i in range(len(b)):
        if b[i]>=2:
            AngleX+= b[i] * math.sin(i * math.pi/6)
            AngleY+= b[i] * math.cos(i * math.pi/6)
    AngleX=round(AngleX,6) #计算坐标转换值
    AngleY=round(AngleY,6)
    if AngleY<0:AngleAddPi=180
    if AngleX<0 and AngleY > 0:AngleAddPi=360
    if AngleX!=0 or AngleY!=0: #参数修正
        if AngleY==0:
            Angle=90 if AngleX>0 else 270 #填补X轴角度
        else:
            Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #计算角度



        if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):



                if Angle>315 and Angle<360:             #处理330-360的角度值

                    Angle=Angle-340
                #Angle=Angle_last*0.3+ Angle*0.7      #低通滤波
                else :
                    if Angle >10 : Angle+=7
                    if Angle >5 and Angle <10 : Angle+=10
                t=get_LB(Angle,5)           #滤波，jd是传入数据，cs是滤波等级，越高越慢，返回滤波后的数据


                if t !=1000 :
                    t1=get_med(t,3)    #再取中位数
                    if t1 !=1000 :
                        if t1>30 : t1=30
                        if t1<-30 : t1=-30
                        Angle=t1
                        Angle_last=Angle
                        lcd.draw_string(60, 200, "Angle: " + str(float(Angle)), lcd.BLUE, lcd.BLACK)
                        lcd.draw_string(60, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)-5), lcd.BLUE, lcd.BLACK)
                        #声源和系统垂直距离是275cm（D区中间）
                        lcd.draw_string(70, 100, "X: " + str((((275/math.cos(-Angle*math.pi/180))**2-75625)**0.5)*(Angle+0.001)/abs(Angle+0.001)-8), lcd.BLUE, lcd.BLACK)
                        lcd.draw_string(70, 120, "Y: " + str(275), lcd.BLUE, lcd.BLACK)

                        lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0))    #清空右边的区域
                        lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*165), 15, 15, (0, 255,200))#角度位置实时
                        #Biaozhi2=1   #等于1就不会再进入该函数了


        AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #计算强度
        mic_list.append(AngleX)     #X坐标
        mic_list.append(AngleY)     #Y坐标
        mic_list.append(AngleR)     #强度
        mic_list.append(Angle)      #角度

        #Juli=2.5/math.cos(Angle)
        #mic_list.append(Juli)
        #print(Juli)
    a = mic.set_led(b,(10,10,0))# 配置 RGB LED 颜色值   （从计算的声源方向设置点亮对应的LED灯）
    #lcd.display(0,color=(255,0,0))
    #img1=draw_line(100,0,100,240,color=(255,0,0))
    #lcd.display(img1)

    #lcd._line(150,0,150,240,color=(255,0,0))
    #img.draw_rectangle((30,30,50,50), color = (255, 0, 0))


    return mic_list #返回列表，X坐标，Y坐标，强度，角度
################################下面是只运行一次的程序###############################
lcd.fill_rectangle(46,5, 230, 4, (255, 0, 0))    #上边线
lcd.fill_rectangle(46,5, 4, 230, (255, 0, 0))    #左边线
lcd.fill_rectangle(46,235, 230, 4, (255, 0, 0))    #下边线
lcd.fill_rectangle(276,5, 4, 234, (255, 0, 0))   #右边线
lcd.fill_rectangle(237,5, 4, 234, (255, 0, 0))    #右边线2

lcd.fill_rectangle(0,77, 47, 4, (255, 0, 0))    #左区域上边线
lcd.fill_rectangle(0,154, 47, 4, (255, 0, 0))    #左区域下边线
lcd.fill_rectangle(0,77, 4, 77, (255, 0, 0))    #左区域左边线


HY.value(0)              # 设置红外地线高电平，关闭红外
##############################主循环，###############################################

while True:
   #引脚0和2的电平是01代表第二题，且第二题只会进行一次，返回一个值在屏幕
    if (KG0.value()==0)and(KG1.value()==1) :
       get_mic_dir2()
                                     #引脚0和2的电平是10代表第三题
    if (KG0.value()==1)and(KG1.value()==0) :
       get_mic_dir3()
                                     ##引脚0和2的电平是11代表第四题
    if (KG0.value()==1)and(KG1.value()==1) :
       get_mic_dir4()
       err=JD-pid           #简易PID控制舵机
       pid+=err*0.09
       Servo(S1,-pid)

    #print(KG0.value(),(~KG1.value()+2))
    #get_mic_dir()
    #time.sleep_ms(500)
